
//小车具体实现流程代码部分===============================================


//初始化小车并赋予其初始状态
void my_car_init(){
  stepper_init();       //步进电机引脚初始化
  car_init();           //麦克纳姆小车引脚初始化
  arm_init();           //机械臂引脚初始化设置
  vision_init();        //视觉模块通信协议初始化
  car_stop();           //设置小车初始状态为停止状态
  arm_keep_init_state();//机械臂各个关节舵机初始化角度设置
}


//小车走到二维码区域
void car_moves_to_the_QR_code_area(){
  //小车从出发区域开始后退0.185米
  car_move(car_msg.back, 0.185);
  delay(car_msg.car_action_delay_time);

  //小车从当前位置开始，向左平移0.65 米到二维码区域
  car_move(car_msg.transleft, 0.65);
  delay(car_msg.car_action_delay_time);
}


//小车开始识别二维码,并将二维码信息保存下来
void car_detect_qr(){
  //如果程序中已打开DEBUG_CAR_FIXED_POINT(小车定点位置测试变量),则执行
  #ifdef DEBUG_CAR_FIXED_POINT
    arm_move_down_angle();//机械臂向下转动一定的角度，使视觉模块能看到二维码
  //否则，执行
  #else
    arm_move_down_angle();//机械臂向下转动一定的角度，使视觉模块能看到二维码
    detect_qr();          //进入二维码检测，直到检测到二维码后，程序才退出
  #endif
}


//小车运动到轮盘区域
void car_move_to_wheel_area(){
  //小车从二维码区域向左平移 0.775米 到码盘区域
  car_move(car_msg.transleft, 0.775);
  delay(car_msg.car_action_delay_time);

  //小车向前移动0.1米，使小车能够更靠近码盘区域
  car_move(car_msg.forward, 0.07);
  delay(car_msg.car_action_delay_time);
}


//小车识别物料区域的物料，并根据二维码顺序抓取物料
void arm_detect_and_catch_material(){
  // car_move(car_msg.turnright, 3);     //右转3度
  //升降模块向上抬升，方便机械臂转动到识别物料颜色的动作
  lift_move(170);
  // delay(car_msg.car_action_delay_time);

  //机械臂转动到识别物料颜色的角度
  arm_move_to_detect_angle(); 
  delay(car_msg.car_action_delay_time);

  //开始判断第一个要抓取的物料对应的颜色代号
  detect_msg.catch_material_flag = true;
  #ifdef DEBUG_CAR_FIXED_POINT
    delay(1000);
  #else
    while(detect_msg.catch_material_flag){
      color_detection_loop();
      if(detect_msg.recv_current_color == detect_msg.qr_code_capture_sequence_one){
      // if(detect_msg.recv_current_color == 1){
        Serial.println("说明此时第一个识别到的颜色物料与二维码第一个要抓取的物料颜色一致");
        detect_msg.catch_material_flag = false;
      }
      else{
        Serial.print("当前识别到的颜色为:"); Serial.print(detect_msg.recv_current_color);Serial.print("  |  ");
        Serial.print("二维码第一个需要抓取的颜色:"); Serial.println(detect_msg.qr_code_capture_sequence_one);
      }
    }
  #endif
    Serial.println("第一个要抓取的物料识别动作完毕");
    servo_move(51, 73, 160, 25, 1500);//delay(1000);  //调整机械臂姿态，防止机械臂和小车前轮干涉
    //升降模块下降100厘米
    lift_move(-100);delay(1000);
    servo_move(51, 73, 160, 90, 1500);//delay(1000); //机械臂抓取物料
    servo_move(51, 139, 160, 90, 1500);//delay(1000);//机械臂抓住物料，其他关节恢复初始状态
    
    lift_move(80);delay(1000);  
    if(detect_msg.recv_current_color == 1){
      servo_move(20, 79, 148, 90, 1500);//delay(1000);//机械臂抓住物料，其他关节恢复初始状态
      servo_move(20, 79, 148, 25, 1500);//delay(1000);//机械臂抓住物料，其他关节恢复初始状态
      servo_move(51, 139, 160, 25, 1500);//delay(1000);//机械臂抓住物料，其他关节恢复初始状态      
    }
    lift_move(-150);delay(1000);
}



//小车走到物料放置区域
void car_moves_to_the_material_put_area(){
  car_move(car_msg.back, 0.06);
  car_move(car_msg.transleft, 0.40);
  car_move(car_msg.back, 0.962);  //0.15
  // car_move(car_msg.turnleft, 91);     //左转90度
  // lift_move(100);delay(1000);            //上升100
  lift_move(45);
  //机械臂调整角度，准备检测位置是否准确
  servo_move(115, 139, 160, 25, 1000);//delay(1000);
  servo_move(115, 80, 160, 25, 1000);//delay(1000);

  /*
  // servo_move(117, 139, 160, 90, 1500);//机械臂抓住物料，其他关节恢复初始状态
  // servo_move(117, 73, 160, 90, 1500);  //机械臂识别颜色动作
  car_move(car_msg.forward, 0.15);   //0.15
  car_move(car_msg.forward, 0.15);   //0.15

  // car_move(car_msg.forward, 0.04);   //0.15

  // servo_move(117, 73, 160, 90, 1500);delay(1000);  //调整机械臂姿态，防止机械臂和小车前轮干涉
  // lift_move(-100);delay(1000);
  // servo_move(117, 73, 160, 20, 1500);delay(1000);  //机械臂抓取物料
  // servo_move(117, 139, 159, 20, 1500);//机械臂抓住物料，其他关节恢复初始状态


  // servo_move(117, 73, 160, 20, 1500);delay(1000);  //机械臂抓取物料
  // servo_move(117, 73, 160, 90, 1500);//机械臂抓住物料，其他关节恢复初始状态
  // servo_move(117, 139, 160, 90, 1500);//机械臂抓住物料，其他关节恢复初始状态
  // car_move(car_msg.back, 0.04);     //0.15


  //左平移1.053米
  car_move(car_msg.back, 1.0);   //0.15
  //后退0.790
  car_move(car_msg.transright, 0.79);   //0.15
  // car_move(car_msg.turnleft, 91);     //左转90度
  // lift_move(100);delay(1000);         //上升100
  // servo_move(180, 139, 160, 90, 1500);//机械臂抓住物料，其他关节恢复初始状态
  // servo_move(180, 73, 160, 90, 1500);  //机械臂识别颜色动作

  // car_move(car_msg.forward, 0.04);   //0.15

  // servo_move(177, 73, 160, 90, 1500);delay(1000);  //调整机械臂姿态，防止机械臂和小车前轮干涉
  // lift_move(-100);delay(1000);
  // servo_move(177, 73, 160, 20, 1500);delay(1000);  //机械臂抓取物料
  // servo_move(51, 139, 160, 20, 1500);delay(1000);//机械臂抓住物料，其他关节恢复初始状态

  // car_move(car_msg.back, 0.04);   //0.15
  //1.01
  car_move(car_msg.transright, 1.01);   //0.15
  //2.04
  // car_move(car_msg.turnright, 5);     //左转90度
  car_move(car_msg.forward, 2.24);       //0.15
  car_move(car_msg.transleft, 0.1);
  
  */
}

bool put_colorring_flag = false;
//小车开始放置色环
void put_colorring_action(){
  Serial.println("小车开始放置物料");
  put_colorring_flag = false;
  car_move(car_msg.transleft, 0.1);
  //机械臂变形，方便升降模块上升
  servo_move(115, 139, 160, 25, 100);
  lift_move(105);
  //机械臂开始抓取红色物块
  servo_move(20, 79, 148, 25, 1500);
  servo_move(20, 79, 148, 90, 1500);
  servo_move(51, 139, 160, 90, 1500);
  //升降模块准备下降，方便机械臂放置物块
  lift_move(-105);
  //机械臂准备放置物块
  servo_move(115, 80, 160, 90, 1000);
  servo_move(115, 80, 160, 25, 1000);
  //机械臂回复初始状态
  servo_move(115, 135, 160, 25, 1000);
  lift_move(-45);
  // car_move(car_msg.transright, 0.1);

  //机械臂开始再次抓取物料
  //lift_move(45);
  lift_move(150);
  servo_move(115, 80, 160, 25, 100);
  lift_move(-105);
  servo_move(115, 80, 160, 90, 100);
  servo_move(115, 137, 160, 90, 100);
  lift_move(105);
  servo_move(20, 79, 148, 90, 1500);
  servo_move(20, 79, 148, 25, 1500);
  servo_move(51, 139, 160, 25, 1500);
  lift_move(-150);
  car_move(car_msg.transright, 0.1);
}

//小车校准位置，检测色环并放置色环
void car_judge_and_detect_colorring(){
  // servo_move(51, 139, 160, 25, 100);//delay(1000);
  // lift_move(45);//delay(1000);
  // servo_move(115, 139, 160, 25, 100);//delay(1000);
  // servo_move(115, 80, 160, 25, 100);//delay(1000);

  put_colorring_flag = true;
  int car_current_position = 0;
  //通过色环处的线来初步校准小车的位置
  calibrate_the_car_through_line();
  Serial.println("直线校准结束");
  // servo_move(115, 80, 160, 25, 100);
  // servo_move(115, 80, 160, 45, 100);
  // servo_move(115, 80, 160, 25, 100);
  //通过色环处的圆来初步校准小车的位置
  calibrate_the_car_through_circle();
  Serial.println("圆环校准结束");
  // servo_move(115, 80, 160, 25, 100);
  // servo_move(115, 80, 160, 45, 100);
  servo_move(115, 76, 160, 25, 200);
  // colorring_detection_loop();  //第一次检测色环
  // detect_msg.receive_current_colorring
  // Serial.print("第一次检测到的色环颜色为:");Serial.println(detect_msg.receive_current_colorring);
  //第一次检测色环与二维码色块是否一致
  // Serial.println("data");
  while(put_colorring_flag){
    //如果检测到的第一个色环与二维码的第一个数字一致
    Serial.println("i am colorring loop");
    colorring_detection_loop();  //第一次检测色环
    Serial.print("第一次检测到的色环颜色为:");Serial.println(detect_msg.receive_current_colorring);
    if(detect_msg.receive_current_colorring == detect_msg.qr_code_capture_sequence_one){
    // if(detect_msg.receive_current_colorring == 1){
      Serial.println("judge the first colorring");
      car_current_position = 3;//小车当前在位置3
      put_colorring_action();
    }
    else{
      Serial.println("judge the second colorring");
      // servo_move(115, 76, 160, 25, 100);
      car_move(car_msg.forward, 0.15);//小车向前移动0.15米
      colorring_detection_loop();     //第二次检测色环
      Serial.print("第二次检测到的色环颜色为:");Serial.println(detect_msg.receive_current_colorring);
      //如果检测到的第二个色环与二维码的第一个数字一致
      if(detect_msg.receive_current_colorring == detect_msg.qr_code_capture_sequence_one){
      // if(detect_msg.receive_current_colorring == 1){
        Serial.println("judge the second colorring");
        car_current_position = 2;//小车当前在位置2
        put_colorring_action();
      }
      else{
        Serial.println("judge the third colorring");
        car_move(car_msg.forward, 0.15);//小车向前移动0.15米
        // colorring_detection_loop();     //第三次检测色环
        // Serial.print("第三次检测到的色环颜色为:");Serial.println(detect_msg.receive_current_colorring);
        car_current_position = 1;//小车当前在位置1
        put_colorring_action();
      }
    }
  }
  Serial.println("小车放置色环动作结束");
}

void put_colorring_action2(){
  Serial.println("小车开始放置物料");
  put_colorring_flag = false;
  car_move(car_msg.back, 0.08);
  //机械臂变形，方便升降模块上升
  servo_move(177, 139, 160, 25, 1000);
  lift_move(105);
  //机械臂开始抓取红色物块
  servo_move(51, 139, 160, 25, 1000);
  servo_move(20, 79, 148, 25, 1500);
  servo_move(20, 79, 148, 90, 1500);
  servo_move(51, 139, 160, 90, 1500);
  servo_move(177, 139, 160, 90, 1500);
  //升降模块准备下降，方便机械臂放置物块
  lift_move(-105);
  //机械臂准备放置物块
  servo_move(177, 80, 160, 90, 1000);
  servo_move(177, 80, 160, 25, 1000);
  servo_move(177, 139, 160, 25, 1000);
  //机械臂回复初始状态
  servo_move(51, 135, 160, 25, 1000);
  lift_move(-45);
  car_move(car_msg.forward, 0.08);
}

//小车移动到第二个色环区域
void car_move_to_second_colorring_area(){
  car_move(car_msg.back, 1.0);   //0.15
  car_move(car_msg.transright, 0.7);   //0.15
  //car_move(car_msg.back, 0.10);  //0.15
  lift_move(45);
  servo_move(177, 139, 160, 25, 2000);
  servo_move(177, 80, 160, 25, 2000);
  calibrate_the_car_through_circle2();
  put_colorring_flag = true;
  int car_current_position = 0;
  servo_move(177, 76, 160, 25, 500);
  while(put_colorring_flag){
    //如果检测到的第一个色环与二维码的第一个数字一致
    Serial.println("i am colorring loop");
    // colorring_detection_loop();  //第一次检测色环
    Serial.print("第一次检测到的色环颜色为:");Serial.println(detect_msg.receive_current_colorring);
    if(detect_msg.receive_current_colorring == detect_msg.qr_code_capture_sequence_one){
      Serial.println("judge the first colorring");
      car_current_position = 3;//小车当前在位置3
      put_colorring_action2();
    }
    else{
      Serial.println("judge the second colorring");
      // servo_move(115, 76, 160, 25, 100);
      car_move(car_msg.transright, 0.15);//小车向前移动0.15米
      colorring_detection_loop();     //第二次检测色环
      Serial.print("第二次检测到的色环颜色为:");Serial.println(detect_msg.receive_current_colorring);
      //如果检测到的第二个色环与二维码的第一个数字一致
      if(detect_msg.receive_current_colorring == detect_msg.qr_code_capture_sequence_one){
      // if(detect_msg.receive_current_colorring == 1){
        Serial.println("judge the second colorring");
        car_current_position = 2;//小车当前在位置2
        put_colorring_action2();
      }
      else{
        Serial.println("judge the third colorring");
        car_move(car_msg.transright, 0.15);//小车向前移动0.15米
        // colorring_detection_loop();     //第三次检测色环
        // Serial.print("第三次检测到的色环颜色为:");Serial.println(detect_msg.receive_current_colorring);
        car_current_position = 1;//小车当前在位置1
        put_colorring_action2();
      }
    }
  }
  //while(1){delay(1000);}
}

void car_move_to_start_area(){
  car_move(car_msg.transright, 1.12);
  car_move(car_msg.forward, 1.80);
  while(1){delay(1000);}
}

